#include "app_task.h"
#include "platform_transmit.h"
#include "data_transmit.h"
#include "protocol.h"
#include "statistics.h"
#include "GRB_lights.h"
#include "bsp_spi.h"
#include "log.h"

#include "remote.h"


ControlInfo_TypeDef control_info = {
	.servo_angle = {0,0,0,0},
	.motor_speed_ref = 0,
	.motor_output = 0,
	.exp_roll = 0.f,
	.exp_pitch = 0.f,
	.rc_status = 1,
};

#define TASK_MONITOR_ENABLE  /*if want to calculate the task time , recomment the macro*/
#ifdef TASK_MONITOR_ENABLE
//#define MAX_TASK_CNT 20
CalTaskTime_TypeDef task_monitor[MAX_TASK_CNT] = {0};

void task_monitor_set_timeflag(void)
{
	for(uint8_t j_index=0;j_index<MAX_TASK_CNT;j_index++)
	{
		task_monitor[j_index].onesec_flag = 1;
	}
}
	
//return the task monitor index
static int8_t task_monitor_add(const char *name)
{
	static uint8_t monitor_task_cnt = 0;

	if( monitor_task_cnt >= MAX_TASK_CNT)
	{
		return -1;
	}

	if(name == NULL )
	{
		return -2;
	}

	task_monitor[monitor_task_cnt].taskName = name;
	task_monitor[monitor_task_cnt].average_tick = 0;
	task_monitor[monitor_task_cnt].max_tick = 0;
	task_monitor[monitor_task_cnt].min_tick = 0;
	task_monitor[monitor_task_cnt].tick_data[0]=0;
	task_monitor[monitor_task_cnt].tick_data[1]=0;
	task_monitor[monitor_task_cnt].max_num = 0;
	task_monitor[monitor_task_cnt].min_num = 0;
	task_monitor[monitor_task_cnt].average_num = 0;
	task_monitor[monitor_task_cnt].num_data = 0;
	task_monitor[monitor_task_cnt].onesec_flag = 0;
	return monitor_task_cnt++;
}

static int8_t task_monitor_start(uint8_t index)
{
	static uint8_t is_init[MAX_TASK_CNT] = {0};
	
	if(index >= MAX_TASK_CNT)
	{
		//invalid index exceed the max cnt
		return -1;
	}
	if( is_init[index] == 0 )
	{
		// first time initial
		task_monitor[index].average_tick = 0;
		task_monitor[index].max_tick = 0;
		task_monitor[index].min_tick = INT32_MAX;
		task_monitor[index].max_num = 0;
		task_monitor[index].min_num = INT32_MAX;
		task_monitor[index].average_num = 0;
		task_monitor[index].onesec_flag = 0;
		is_init[index] = 1;
	}
	//task_monitor[index].tick_data[0] = HAL_GetTick();
	task_monitor[index].tick_data[0] = CalTaskTime_GetTick();
	return 0;
}

static int8_t task_monitor_end(uint8_t index)
{
	uint32_t tick;
	if(index >= MAX_TASK_CNT)
	{
		//invalid index exceed the max cnt
		return -1;
	}
	//task_monitor[index].tick_data[1]	= HAL_GetTick();
	task_monitor[index].tick_data[1] = CalTaskTime_GetTick();
	if( task_monitor[index].tick_data[0] <= task_monitor[index].tick_data[1])
	{
		tick = task_monitor[index].tick_data[1] - task_monitor[index].tick_data[0];
	}else{
		tick = 20000 - task_monitor[index].tick_data[0] + task_monitor[index].tick_data[1];
	}
		
	if( task_monitor[index].max_tick < tick )
	{
		task_monitor[index].max_tick = tick;
	}
	if( task_monitor[index].min_tick > tick )
	{
		task_monitor[index].min_tick = tick;
	}
	task_monitor[index].average_tick = 0.5f * task_monitor[index].average_tick + 0.5f * tick;
	
	
	task_monitor[index].num_data ++;
	
	if(task_monitor[index].onesec_flag)
	{
		if( task_monitor[index].max_num < task_monitor[index].num_data )
		{
			task_monitor[index].max_num = task_monitor[index].num_data;
		}
		if( task_monitor[index].min_num > task_monitor[index].num_data )
		{
			task_monitor[index].min_num = task_monitor[index].num_data;
		}
		task_monitor[index].average_num = 0.5f * task_monitor[index].average_num + 0.5f * task_monitor[index].num_data;
		
		task_monitor[index].onesec_flag = 0;
		task_monitor[index].num_data = 0;
	}
	
	return 0;
}

static int8_t get_index_by_name(const char *name)
{
	int i = 0;
	if( name == NULL )
	{
		return -1;
	}
	for( i = 0;i < MAX_TASK_CNT;i++)
	{
		if(task_monitor[i].taskName == NULL )
		{
			return -2;
			//can not find the monitor
		}

		if( strcmp(name,task_monitor[i].taskName) == 0 )
		{
			return i;
		}
	}
	return -2;
	//loop done, cannot find the monitor
}
#endif


extern TofInfo_TypeDef tof_info;
extern SensorInfo_TypeDef sensor_info;
extern float roll_init;
extern float pitch_init;		
extern float yaw_init;		
extern float height_init;	

osThreadId_t attitudeSolutionTaskHandle;
const osThreadAttr_t attitudeSolutionTask_attributes = {
  .name = "attitudeSolutionTask\0",
  .stack_size = 256 * 4,
  .priority = (osPriority_t) osPriorityNormal,
};
	

void AttitudeSolution_Task(void *arg)
{
	UNUSED(arg);
	
	for(;;)
	{
		
		BMI008_GetData(0.002f);
		AK09915_GetData();
		SPL06_GetHeight();	
		
		Attitude_MahonyAHRSupdate( sensor_info.status_data.gyro[0],sensor_info.status_data.gyro[1],sensor_info.status_data.gyro[2],
																sensor_info.status_data.accel[0],sensor_info.status_data.accel[1],sensor_info.status_data.accel[2],
																0,0,0);		
																// sensor_info.status_data.magn[0],sensor_info.status_data.magn[1],sensor_info.status_data.magn[2]); 
		RollPitch_InitCalibration();
		
		
#ifdef BARO_HEIGHT_USED
		Baro_Fusion();		//Baro - Get height data
#else
		Fusion();			//MTF - Get height data
#endif
		
		osDelay(2);
	}
}


osThreadId_t control50msTaskHandle;
const osThreadAttr_t control50msTask_attributes = {
  .name = "control50msTask\0",
  .stack_size = 256 * 4,
  .priority = (osPriority_t) osPriorityNormal,
};


extern uint8_t height_control_mode;
extern uint8_t take_off_mode;
extern TIM_HandleTypeDef htim5;
extern incremental_pid_t motor_speed_ipid;
extern MTF02PackInfo_TypeDef mtf_info;


void Control50ms_Task(void *arg)
{
	UNUSED(arg);
	
//	uint8_t start_flag = 1;
//	uint32_t ready_cnt = 0;
//	
//	
//	float rc_throttle = 0;
//	float rc_expms = 0;
//	float rc_exp_attitude[2] = {0};
//	float rc_exp_flight[4] = {0};
//	uint8_t last_status = 0;
//	uint8_t status = 0;
//	
//	uint8_t boost_flag = 0;
//	float ready_motorspeed = 0;
//	float sin_value = 0;
//	float t = 0;
//	float w = 6.28f / 4.f;
//	const float base_height = 25.f;
//	uint8_t experiment_type = 5;  //1-throttle 2-motor_speed 3-attitude_control 4-velocity control 5-height_control 6-motor_speed control
	for(;;)
	{	
////		Heating_Control();
//		
		MotorSpeed_Calculate();
		//calculate motor speed;
//		
		if( control_info.rc_status == 0) //remote controler online
		{
//			if( experiment_type == 1 )
//			{
//				get_throttle(&rc_throttle);			
//				control_info.motor_output = rc_throttle;
//				//throttle experiment				
//			}
//			else if( experiment_type == 2 )
//			{
//				status = get_expms( &rc_expms );
//				if( status == 1 && status != last_status )
//				{
//					start_flag = 1;
//				}
//				last_status = status;
//				//speed control mode				
//				const float max_motor_speed = 8000;
//				
//				if( status == 1 )
//				{
//					if( start_flag == 1 )
//					{
//						//start stage
//						ready_cnt += 50;
//						
//						control_info.motor_output = 150;
//						
//						if( ready_cnt >= 2000 )
//						{
//							t = 0;
//							start_flag = 0;
//							ready_cnt = 0;
//							motor_speed_ipid.output = control_info.motor_output;
//							sin_value = 0;
//						}
//					}
//					else
//					{
//						sin_value = max_motor_speed /4.f * sin(w*t) + max_motor_speed /4.f;
//						t+=0.05f;
//						if( t <= 4.f )
//						{
//							sin_value = 3000;
//						}
//						else if( t <= 8.f && t > 4.f )
//						{	
//							sin_value = 6000;
//						}						
//						else if( t <= 12.f && t > 8.f )
//						{	
//							sin_value = 9000;
//						}
//						else if( t <= 16.f && t > 12.f )
//						{	
//							sin_value = 12000;
//						}
//						else if( t <= 20.f && t > 16.f )
//						{	
//							sin_value = 9000;
//						}			
//						else if( t <= 24.f && t > 20.f )
//						{	
//							sin_value = 6000;
//						}									
//						else
//						{	
//							sin_value = 3000;
//							t = 0;
//						}
//						
//						control_info.motor_speed_ref = sin_value + 2000;
//		//					control_info.motor_speed_ref = expect * max_motor_speed + 5000;
//						control_info.motor_output = motorspeed_control();
//					}
//				}
//				else
//				{
//					motor_speed_ipid.output = 0;
//					control_info.motor_output = 0;
//					control_info.motor_speed_ref = 0;
//				}
//			}
//			else if( experiment_type == 3 )
//			{
//				//attitude control experiment
//				status = get_expms_exprotate(rc_exp_attitude);
//				
//				if( status == 1 && status != last_status )
//				{
//					start_flag = 1;
//				}
//				last_status = status;
//				
//				if( status == 1 )
//				{
//					if( start_flag == 1 )
//					{
//						//start stage
//						ready_cnt += 50;						
//						control_info.motor_output = 150;						
//						if( ready_cnt >= 2000 )
//						{
//							t = 0;
//							start_flag = 0;
//							ready_cnt = 0;
//							motor_speed_ipid.output = control_info.motor_output;
//							sin_value = 0;
//						}
//					}
//					else
//					{
//						control_info.exp_pitch = rc_exp_attitude[1];
//						control_info.motor_speed_ref = rc_exp_attitude[0] + 4000;
//						control_info.motor_output = motorspeed_control();
//					}
//				}
//				else
//				{
//					control_info.exp_pitch = 0;
//					motor_speed_ipid.output = 0;
//					control_info.motor_output = 0;
//					control_info.motor_speed_ref = 0;
//				}
//			}
//			else if( experiment_type == 4 ) //velocity control experiment
//			{
//				
//				status = get_expheight( rc_exp_flight,0);
//				
//				control_info.exp_roll = sensor_info.init_data.init_roll + rc_exp_flight[1];
//				control_info.exp_pitch = sensor_info.init_data.init_pitch + rc_exp_flight[2];
//				control_info.exp_yaw = sensor_info.init_data.init_yaw + rc_exp_flight[3];
//				
//				//0-velocity control 1-height control
//				if( status == 1 && status != last_status )
//				{
//					start_flag = 1;
//					ready_cnt = 0;
//					boost_flag = 0;
//				}
//				last_status = status;
//				
//				if( status == 1 )
//				{
//					if( start_flag == 1 )
//					{
//						//start stage					
//						if( ready_cnt >= 2000 )
//						{
//							if(boost_flag == 0)
//							{
//								boost_flag = 1;
//								control_info.motor_speed_ref = sensor_info.filter_data.lpf_motor_speed[0];
//								motor_speed_ipid.output = control_info.motor_output;
//							}
//						}
//						else
//						{
//							control_info.motor_output = 150;
//							ready_cnt += 50;
//						}

//						if( boost_flag == 1 )
//						{
//							control_info.motor_speed_ref += 120.f;			
//							control_info.motor_output = motorspeed_control();
//							
//							if( sensor_info.filter_data.lpf_motor_speed[0] >= 12000.f )
//							{
//								boost_flag = 0;
//								start_flag = 0;
//								//exit
//							}
//						}
//					}
//					else
//					{
//						control_info.exp_velocity_z = rc_exp_flight[0];
//						
//						float tmp_ms = Height_Control( &control_info );
//						
//						if( tmp_ms <= 4000 )
//						{
//							control_info.motor_speed_ref = 4000;
//						}
//						else if( tmp_ms >= 17000 )
//						{
//							control_info.motor_speed_ref = 17000;
//						}
//						else
//						{
//							control_info.motor_speed_ref = tmp_ms;
//						}
//						control_info.motor_output = motorspeed_control();
//					}
//				}
//				else
//				{
//					sensor_info.init_data.init_yaw = sensor_info.euro_angle.yaw;
//					//current angle for reference
//					control_info.exp_velocity_z = 0;
//					motor_speed_ipid.output = 0;
//					control_info.motor_output = 0;
//					control_info.motor_speed_ref = 0;
//				}
//			}
//			else if( experiment_type == 5 ) //height control experiment
//			{
//				status = get_expheight( rc_exp_flight,1);
//				
//				control_info.exp_roll = sensor_info.init_data.init_roll + rc_exp_flight[1];
//				control_info.exp_pitch = sensor_info.init_data.init_pitch + rc_exp_flight[2];
//				control_info.exp_yaw = sensor_info.init_data.init_yaw + rc_exp_flight[3];
//				
//				//0-velocity control 1-height control
//				if( status == 1 && status != last_status )
//				{
//					start_flag = 1;
//					ready_cnt = 0;
//					boost_flag = 0;
//				}
//				last_status = status;
//				
//				if( status == 1 )
//				{
//					if( start_flag == 1 )
//					{
//						//start stage					
//						if( ready_cnt >= 2000 )
//						{
//							if(boost_flag == 0)
//							{
//								boost_flag = 1;
//								control_info.motor_speed_ref = sensor_info.filter_data.lpf_motor_speed[0];
//								motor_speed_ipid.output = control_info.motor_output;
//							}
//						}
//						else
//						{
//							control_info.motor_output = 150;
//							ready_cnt += 50;
//						}

//						if( boost_flag == 1 )
//						{
//							control_info.motor_speed_ref += 120.f;			
//							control_info.motor_output = motorspeed_control();
//							
//							if( sensor_info.filter_data.lpf_motor_speed[0] >= 12000.f )
//							{
//								boost_flag = 0;
//								start_flag = 0;
//								//exit
//							}
//						}
//					}
//					else
//					{
//						
//						control_info.exp_height = base_height + rc_exp_flight[0];
//						
//						float tmp_ms = Height_Control( &control_info );
//						
//						if( tmp_ms <= 4000 )
//						{
//							control_info.motor_speed_ref = 4000;
//						}
//						else if( tmp_ms >= 17000 )
//						{
//							control_info.motor_speed_ref = 17000;
//						}
//						else
//						{
//							control_info.motor_speed_ref = tmp_ms;
//						}
//						control_info.motor_output = motorspeed_control();
//					}
//				}
//				else
//				{
//					sensor_info.init_data.init_yaw = sensor_info.euro_angle.yaw;
//					//current angle for reference
//					control_info.exp_height = base_height;
//					control_info.exp_velocity_z = 0;
//					motor_speed_ipid.output = 0;
//					control_info.motor_output = 0;
//					control_info.motor_speed_ref = 0;
//				}
//			}
//			else if( experiment_type == 6 ) //motor speed control experiment
//			{
////				static float time = 0;
//				status = get_expheight( rc_exp_flight,2);
//				
////				control_info.exp_gyrox = rc_exp_flight[1];
////				time += 0.05f;
////				control_info.exp_roll = 10.f * sin( time );
////				control_info.exp_roll = 15.f;
////				control_info.exp_roll = sensor_info.init_data.init_roll + rc_exp_flight[1];
//				control_info.exp_pitch = sensor_info.init_data.init_pitch + rc_exp_flight[2];
//				control_info.exp_gyroz = rc_exp_flight[3];
////				control_info.exp_yaw = sensor_info.init_data.init_yaw + rc_exp_flight[3];
//				//0-velocity control 1-height control
//				if( status == 1 && status != last_status )
//				{
//					start_flag = 1;
//				}
//				last_status = status;
//				
//				if( status == 1 )
//				{
//					if( start_flag == 1 )
//					{
//						//start stage
//						ready_cnt += 50;						
//						control_info.motor_output = 150;						
//						if( ready_cnt >= 2000 )
//						{
//							t = 0;
//							start_flag = 0;
//							ready_cnt = 0;
//							motor_speed_ipid.output = control_info.motor_output;
//							sin_value = 0;
//						}
//					}
//					else
//					{
//						float tmp_ms = rc_exp_flight[0];
//						
//						if( tmp_ms <= 4000 )
//						{
//							control_info.motor_speed_ref = 4000;
//						}
//						else if( tmp_ms >= 17000 )
//						{
//							control_info.motor_speed_ref = 17000;
//						}
//						else
//						{
//							control_info.motor_speed_ref = tmp_ms;
//						}
//						control_info.motor_output = motorspeed_control();	
//					}
//				}
//				else
//				{
//					sensor_info.init_data.init_roll = sensor_info.euro_angle.roll;
//					sensor_info.init_data.init_pitch = sensor_info.euro_angle.pitch;
//					sensor_info.init_data.init_yaw = sensor_info.euro_angle.yaw;
//					//current angle for reference
//					motor_speed_ipid.output = 0;
//					control_info.motor_output = 0;
//					control_info.motor_speed_ref = 0;
//				}
//			}
			uint8_t mode_e = Experiment_SetMode();
			if(mode_e == 1)
			{
				control_info.motor_output = motorspeed_control();
			}
			else if(mode_e == 2)
			{
				Height_Control( &control_info );
				control_info.motor_output = motorspeed_control();
			}
		}
		else //remote controler offline
		{
			control_info.motor_output = 0;
			control_info.motor_speed_ref = 0;
		}
	
		Motor_Output(control_info.motor_output);
		
		
		
		osDelay(50);
	}
}  



osThreadId_t control2msTaskHandle;
const osThreadAttr_t control2msTask_attributes = {
  .name = "control2msTask\0",
  .stack_size = 512 * 4,
  .priority = (osPriority_t) osPriorityLow,
};
void Control2ms_Task(void *arg)
{
	UNUSED(arg);
	float reset_angle[4] = {0,0,0,0};
	uint8_t buf[MAX_QUEUE_PACK_SIZE];
	uint32_t len;	

	for(;;)
	{
		if ( Attitude_SetDisable() != 1)
		{
			Attitude_Control(&control_info);	
		}
		
		if( control_info.rc_status == 0 )
		{
			Servo_AngleControl(control_info.servo_angle);
		}
		else
		{
			Servo_AngleControl(reset_angle);
		}
		
		len = HC_DataDequeue(buf);			
		digital_twin_send_data(buf,len);
		
		osDelay(2);
	}
}




osThreadId_t remoteControlTaskHandle;
const osThreadAttr_t remoteControlTask_attributes = {
  .name = "remoteControlTask\0",
  .stack_size = 256 * 4,
  .priority = (osPriority_t) osPriorityNormal,
};


void RemoteControl_Task(void *arg)
{
	UNUSED(arg);	
	
	for(;;)
	{	
		Update_Remote_Mode(0.05);
		osDelay(50);
	}
}
extern float servo_torque[4];

extern Pid_TypeDef pid_velocity_z;
extern Pid_TypeDef pid_height;


osThreadId_t dataTransmitTaskHandle;
const osThreadAttr_t dataTransmitTask_attributes = {
  .name = "dataTransmitTask\0",
  .stack_size = 512 * 4,
  .priority = (osPriority_t) osPriorityNormal,
};

extern float ret_exp_rpm,calculate_lift;

void DataTransmit_Task(void *arg)
{
	float transmit_data[20] = {0};
	uint16_t ecu_data[14] = {0};
	uint16_t data_rc[16];
	
	uint8_t dt_sensor_buf[sizeof(protocol_sensor_info_t)] = {0};
	uint32_t dt_sensor_len = 0;
	
	uint8_t dt_input_buf[sizeof(protocol_input_info_t)] = {0};
	uint32_t dt_input_len = 0;
	
		uint8_t dt_expect_buf[sizeof(protocol_expect_info_t)] = {0};
	uint32_t dt_expect_len = 0;
	UNUSED(arg);
		
	
	for(;;)
	{	
//		transmit_data[0] = control_info.exp_height;
//		transmit_data[1] = sensor_info.fusion_data.position[2];
//		transmit_data[2] = control_info.exp_velocity_z;
//		transmit_data[3] = sensor_info.fusion_data.speed[2];
//		transmit_data[4] = sensor_info.fusion_data.accel[2];
//		transmit_data[5] = control_info.motor_speed_ref;
//		transmit_data[6] = sensor_info.filter_data.lpf_motor_speed[0];
//		transmit_data[7] = ret_exp_rpm;
//		transmit_data[8] = calculate_lift;		
		transmit_data[0] = control_info.exp_gyroz;
		transmit_data[1] = sensor_info.status_data.gyro[2];
//		transmit_data[3] = sensor_info.raw_data.of_speed_x;
//		transmit_data[4] = sensor_info.raw_data.of_speed_y;
		transmit_data[2] = control_info.servo_angle[3];
		transmit_data[3] = control_info.servo_angle[1];
		transmit_data[4] = control_info.servo_angle[2];
		transmit_data[5] = control_info.servo_angle[0];
		transmit_data[6] = control_info.motor_speed_ref;
		transmit_data[7] = control_info.motor_output;

//		transmit_data[7] = sensor_info.fusion_data.position[0];		
//		transmit_data[8] = sensor_info.fusion_data.position[1];		
//		transmit_data[9] = sensor_info.kf_data.position[2];	
		HostComputer_SendUserData_float(FUNC_ID1,transmit_data,10); 
		
		float q_tmp[4];
		MahonyAHRS_GetQuaternion(q_tmp);
		protocol_sensor_info_pack_t sensor_pack;
		sensor_pack.accel_x = sensor_info.status_data.accel[0];
		sensor_pack.accel_y = sensor_info.status_data.accel[1];
		sensor_pack.accel_z = sensor_info.status_data.accel[2];
		sensor_pack.gyro_x = sensor_info.status_data.gyro[0];
		sensor_pack.gyro_y = sensor_info.status_data.gyro[1];
		sensor_pack.gyro_z = sensor_info.status_data.gyro[2];
		sensor_pack.roll = sensor_info.euro_angle.roll;
		sensor_pack.pitch = sensor_info.euro_angle.pitch;
		sensor_pack.yaw = sensor_info.euro_angle.yaw;
		sensor_pack.vel_x = sensor_info.fusion_data.speed[0];
		sensor_pack.vel_y = sensor_info.fusion_data.speed[1];
		sensor_pack.vel_z = sensor_info.fusion_data.speed[2];
		sensor_pack.pos_x = sensor_info.fusion_data.position[0];
		sensor_pack.pos_y = sensor_info.fusion_data.position[1];
		sensor_pack.pos_z = sensor_info.fusion_data.position[2];
		sensor_pack.q0 = q_tmp[0];
		sensor_pack.q1 = q_tmp[1];
		sensor_pack.q2 = q_tmp[2];
		sensor_pack.q3 = q_tmp[3];
		sensor_pack.voltage = sensor_info.power_voltage;
		
		dt_sensor_len = protocol_sensor_info_pack(dt_sensor_buf,sizeof(dt_sensor_buf),&sensor_pack);
		//HC_DataInqueue(dt_sensor_buf,dt_sensor_len);
		
		protocol_input_info_pack_t input_pack;
		input_pack.servo_angle[0] = control_info.servo_angle[0];
		input_pack.servo_angle[1] = control_info.servo_angle[1];
		input_pack.servo_angle[2] = control_info.servo_angle[2];
		input_pack.servo_angle[3] = control_info.servo_angle[3];
		input_pack.motor_speed = sensor_info.filter_data.lpf_motor_speed[0];
		
		dt_input_len = protocol_input_info_pack(dt_input_buf,sizeof(dt_input_buf),&input_pack);
		//HC_DataInqueue(dt_input_buf,dt_input_len);
		
		protocol_expect_info_pack_t expect_pack;
		//euro angle
		expect_pack.roll = control_info.exp_roll;
		expect_pack.pitch = control_info.exp_pitch;
		expect_pack.yaw = control_info.exp_yaw;
		//gyro
		expect_pack.gyrox = control_info.exp_gyrox;
		expect_pack.gyroy = control_info.exp_gyroy;
		expect_pack.gyroz = control_info.exp_gyroz;
		//motor speed
		expect_pack.motor_speed = control_info.motor_speed_ref;
		//position
		expect_pack.x_pos = control_info.exp_position_x;
		expect_pack.y_pos = control_info.exp_position_y;
		expect_pack.z_pos = control_info.exp_height;
		//velocity
		expect_pack.x_vel = control_info.exp_velocity_x;
		expect_pack.y_vel = control_info.exp_velocity_y;
		expect_pack.z_vel = control_info.exp_velocity_z;
		
		dt_expect_len = protocol_expect_info_pack(dt_expect_buf,sizeof(dt_expect_buf),&expect_pack);
		//HC_DataInqueue(dt_expect_buf,dt_expect_len);

		osDelay(20);
	}
}



osThreadId_t systemTaskHandle;
const osThreadAttr_t systemTask_attributes = {
  .name = "systemTask\0",
  .stack_size = 512 * 4,
  .priority = (osPriority_t) osPriorityNormal,
};
	
// system monitor task
// 1.power 
// 2.led
void System_Task(void *arg)
{
	uint8_t power_status = 0;
	float power_min_voltage = 22.0;
	UNUSED(arg);		

	for(;;)
	{	
		/* power monitor */
		PowerManager_Task(arg); 
		if( sensor_info.power_voltage <= power_min_voltage)
		{
			power_status = 1;	
		}
		else 
		{
			power_status = 0;
		}
		
		HAL_GPIO_TogglePin(LED_BOARD_GPIO_Port,LED_BOARD_Pin);
		
		if( power_status == 0 )
		{			
			HAL_GPIO_WritePin(LED_R_GPIO_Port,LED_R_Pin,GPIO_PIN_RESET);
			HAL_GPIO_WritePin(LED_B_GPIO_Port,LED_B_Pin,GPIO_PIN_RESET);
			HAL_GPIO_TogglePin(LED_G_GPIO_Port,LED_G_Pin);		
		}
		else 
		{
			HAL_GPIO_WritePin(LED_G_GPIO_Port,LED_G_Pin,GPIO_PIN_RESET);
			HAL_GPIO_WritePin(LED_B_GPIO_Port,LED_B_Pin,GPIO_PIN_RESET);
			HAL_GPIO_TogglePin(LED_R_GPIO_Port,LED_R_Pin);				
		}

		/*sbus keep alive monitor*/
		uint8_t sbus_status = SBUS_KeepAliveTask(300);
		if( sbus_status == 0xff )
		{
			control_info.rc_status = 1;
		}
		else
		{
			control_info.rc_status = 0;
		}
		
		
		osDelay(300);
	} 
}


osThreadId_t statisticsTaskHandle;
const osThreadAttr_t statisticsTask_attributes = {
  .name = "statisticsTask\0",
  .stack_size = 128 * 4,
  .priority = (osPriority_t) osPriorityNormal,
};
	
// system monitor task
// 1.power 
// 2.led
void statistics_task(void *arg)
{
	UNUSED(arg);		
	for(;;)
	{	
	
		statistics_monitor();
		/*statistics monitor*/	
		
		osDelay(2);
	} 
}


void AppTask_Init(void)
{
	DEBUG_PRINTF("application task initial");
	
	systemTaskHandle = osThreadNew(System_Task, NULL, &systemTask_attributes);
	dataTransmitTaskHandle = osThreadNew(DataTransmit_Task, NULL, &dataTransmitTask_attributes);
	remoteControlTaskHandle = osThreadNew(RemoteControl_Task, NULL, &remoteControlTask_attributes);
	control2msTaskHandle = osThreadNew(Control2ms_Task, NULL, &control2msTask_attributes);
	control50msTaskHandle = osThreadNew(Control50ms_Task, NULL, &control50msTask_attributes);
	attitudeSolutionTaskHandle = osThreadNew(AttitudeSolution_Task, NULL, &attitudeSolutionTask_attributes);
//	statisticsTaskHandle = osThreadNew(statistics_task, NULL, &statisticsTask_attributes);
}



